/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "J2B2OccuSLAM.h"

/**
 * Constructor
 * @param size_x the size of the grid in x-direction, in meters
 * @param size_y the size of the grid in y-direction, in meters
 * @param reso   Resolution. The size of one cell in meters
 * @param origin The position of the grid origin in the world frame of reference
 * @param cor_search_area The area used in searching the correlation (i.e. maximum odometry error)
 * @param cor_search_angle The angle used in searching the correlation (i.e. maximum odometry error)
 * @param number_of_scan_meas How many scans there is in the measurement
 */
J2B2OccuSLAM::J2B2OccuSLAM(float size_x, 
								float size_y, 
								float reso, 
								Grid::TCoord2D origin,
								float cor_search_area,
								float cor_search_angle,
								int number_of_scan_meas)
{
		isSLAMEnabled = true;
		O = new TOccupancyGrid(size_x,size_y,reso,origin); ///create new occupancy grid
		O->setGridToValue(0.5);
		gmatcher = new CAHistogramECorrelationScanMatch(cor_search_area,cor_search_angle);
		numofscans = number_of_scan_meas;
		
		meas.allocate(numofscans);
		ref.allocate(numofscans);
		grid_scan.alloc(numofscans);
		virt.alloc(numofscans);
		
		dataMutex = ownMutex_Init();
		
		ASRoboClient = NULL;
		PositionClient = NULL;
		LaserClient = NULL;
		gi = NULL;
		reference_set = false;
		out_x=0;out_y=0;out_a=0; 
}


J2B2OccuSLAM::~J2B2OccuSLAM(){
		
		delete O;
		delete gmatcher;
		ownMutex_Destroy(dataMutex);
}
/**
* Connect to the robot using initialized GIMI interface
* @param *gim initialized gimi pointer
* @param robot_name The name of the robot
**/
bool J2B2OccuSLAM::connectToRobot(gimi::GIMI *gim, std::string robot_name){
		if(!gim){
				dPrint(1,"GIMI NULL!!!\n");
				return false;
		}
		if(!gim->isConnected()){
				dPrint(1,"You must connect the GIMI first!!");
				return false;
		}
		robot = robot_name;
		gi = gim;
		
		if(initializeRobot()){
				RunThread();
				return true;
		}
		return false;
}
/**
		* Connect to the robot by initializing own GIMI
		* @param hubhost name of the hubhost
		* @param hubport The hub port number_of_scan_meas
		* @param robot_name the name of the robot
		*/
bool J2B2OccuSLAM::connectToRobot(std::string hubhost, int hubport, std::string robot_name){
		robot = robot_name;
		gi = new gimi::GIMI();
		bool result = gi->connectToHub(hubhost, hubport);
		if(!result){
				dPrint(1,"Failed to connec to %s:%d",hubhost.c_str(),hubport);
				return false;
		}
		if(initializeRobot()){
				fprintf(stderr,"Starting thread!");
				RunThread();
				return true;
		}
		return false;
}


void J2B2OccuSLAM::getPosition(float &x, float &y, float &a){
		ownMutex_Lock(dataMutex);
		x = out_x; y=out_y; a = out_a;
		ownMutex_Unlock(dataMutex);
}

/**
		* The thread function - This is started IF ASRobo data source is
		* added with connectToRobot() call
		*/
int J2B2OccuSLAM::ThreadFunction(const int aThreadNumber){
		ASRobo::Drivers::Devices::Ranging::CDistanceArray aDistanceArray;
		ASRobo::Core::CPose aPosition; 
		gim::time aTimestamp;
		
		int seq=0;
		bool result=false;
		bool laser_read = false;
		odo_set=false;
		int badval=0;
		while(1){
				result = false;
				///Do nothing if algorthm is disabled
				if(!isSLAMEnabled){
						ownSleep_ms(100);
						continue;
				}
				
				if (LaserClient){
					int oldseq = seq;
					result = LaserClient->GetDistanceArray(aDistanceArray,&seq,1);
					if(seq == oldseq) result=false;
					
					if(!result){ 
							//fprintf(stderr,"No new data from laser ");
							laser_read = false;
					}else{
						//fprintf(stderr,"Data from laser %d (%d)!!! ",seq,aDistanceArray.distanceArray.size());
						laser_read = true;
						for(unsigned int i=0; i < aDistanceArray.distanceArray.size(); ++i){
								meas.r[i] = aDistanceArray.distanceArray[i].distance;
								meas.a[i] = aDistanceArray.distanceArray[i].heading;				
						}
					}
				}
				
				result = false;
				if(PositionClient){
						result = PositionClient->GetOdometryPosition(aPosition,&aTimestamp,false);	
						if(!result){
								//fprintf(stderr,"No Position :(\n");
						}else {
								//fprintf(stderr,"Position!!!\n");
								odo_cur.set(aPosition.x,aPosition.y,aPosition.heading);	
								if(!odo_set) {odo_ref=odo_cur; odo_set = true;}
								/// There is new measurement
								if(laser_read){
										odo_meas_cur = odo_cur;
										if(!reference_set) odo_meas_ref = odo_meas_cur;
								}
						}
				}
				///Estimated movement between the two latest measurements
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				pos = pos.integrateDifferential(est); ///< this is now the predicted position
				odo_ref = odo_cur;
				///We have measurement
				if(laser_read){
						if(!reference_set){ ///< still initializing
								badval = filterScan(meas,grid_scan);
								//for(int i=0;i<grid_scan.N;i++) fprintf(stderr,"%.2f ",grid_scan.r[i]);
								//fprintf(stderr,"Setting %d bad values to map\n",badval);
								O->setScanFillProb(pos.x, pos.y ,pos.a, grid_scan,0.01,0.99);
								reference_set = true; ///< Now we are ready to slam
						}else{
								O->getScan(pos.x,pos.y, (pos.a-M_PI/2.0), numofscans, M_PI/(numofscans-1), (grid_t) 0.7, virt); ///<getscan
								badval = filterScan(virt, ref);
								if(badval < 30){
										gmatcher->setCurrentPose(pos);
										gmatcher->setReferenceScan(ref,pos); 
										gmatcher->update(meas,pos);    ///update
										gmatcher->getCurrentPose(pos); ///< Get the estimate
										filterScan(meas,grid_scan);
										//fprintf(stderr,"N:(%d, %d, %d) bad=%d\n",grid_scan.N, meas.N,virt.N,badval);
										O->setScanFillProb(pos.x, pos.y ,pos.a, grid_scan,0.3,0.9);
								}
								else{
										//fprintf(stderr,"N:(%d, %d, %d) bad=%d\n",grid_scan.N, meas.N,virt.N,badval);
										filterScan(meas,grid_scan);
										O->setScanFillProb(pos.x, pos.y ,pos.a, grid_scan,0.1,0.9);
								}
						}
				}
				
				setPosition(pos.x,pos.y,pos.a);
				ownSleep_ms(20);
		}
return 0;
}

void J2B2OccuSLAM::reset(){
		pos.set(0,0,0);
		odo_set = false;
		reference_set = false;
		O->setGridToValue(0.5);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
/// PRIVATE METHODS
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
void J2B2OccuSLAM::setPosition(float &x, float &y, float &a){
		ownMutex_Lock(dataMutex);
		out_x=x; out_y=y; out_a=a;
		ownMutex_Unlock(dataMutex);
}

/**
* Initialize the robot control module parts
*/
bool J2B2OccuSLAM::initializeRobot(){
		bool result = false;
		
		ASRoboClient = new CASRoboClient;
		result = ASRoboClient->Initialize(gi, robot, 2000);
		dPrint(3,"ASRobo Init %s", result ? "Successful!" : "Failed!!");
		if(!result) return false;
		
		PositionClient = ASRoboClient->GetPositionClient(0); ///< Get odometry
		LaserClient = ASRoboClient->GetRangingClient(0);     ///< get laser
		
		if(PositionClient==NULL || LaserClient==NULL){
				dPrint(1,"Position or Laser Initialization failed!");
				return false;
		}
		result = LaserClient->Open();
		dPrint(1,"Opening Device Ranging::Laser %s!",result?"OK":"FAILED");
		if(!result) return  false;
		result = PositionClient->Open();
		dPrint(1,"Opening Device Position %s!",result?"OK":"FAILED");
		if(!result) return  false;
		
		dPrint(1,"Robot connection established with '%s'!! Reseting the odometry...",robot.c_str());
		PositionClient->SetOdometryPosition(ASRobo::Core::CPose(0,0,0));
		ownSleep_ms(200);
		return true;
}

/**
* Filter the measurement for the grid
*/ 
int J2B2OccuSLAM::filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}
int J2B2OccuSLAM::filterScan(Grid::TScan &v,ScanMatch::scan &out){
		int bad=0;
		for(int i=0;i<v.N;i++){
				out.a[i]=virt.a[i]-M_PI/2.0;
				out.r[i]=virt.r[i]; 
				if (virt.r[i]<0){out.r[i]=0;bad++;}
		}
		
		return bad;
}






